/*
 * Copyright (c) 2012 Johann Hanne
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

/*
 * BEWARE: Some functions assume little endian!
 * (search for "assume little endian")
 *
 * BEWARE: Some string functions might read one byte beyond the
 * string buffer, which only is ok for systems without MPU/MMU
 */

#include <string.h>
#include <stdint.h>
#include <ctype.h>
#include <limits.h>

/* Functions inlined by wcc when /oi switch is used */
#ifndef __INLINE_FUNCTIONS__

char *strchr(const char *s, int c) {
    const uint16_t *p = (uint16_t *)s;
    const uint16_t c1 = (uint16_t)c & 0x00ff;
    const uint16_t c2 = ((uint16_t)c << 8) & 0xff00;
    uint16_t u16;

    for (;;) {
        u16 = *p;
        /* assume little endian */
        if ((u16 & 0x00ff) == 0x0000) {
            return NULL;
        } else if ((u16 & 0x00ff) == c1) {
            return (char *)p;
        } else if ((u16 & 0xff00) == 0x0000) {
            return NULL;
        } else if ((u16 & 0xff00) == c2) {
            return ((char *)p) + 1;
        } else {
            p++;
        }
    }
}

void *memcpy(void *dest, const void *src, size_t n) {
    const uint16_t *s = (uint16_t *)src;
    uint16_t *d = (uint16_t *)dest;
    uint16_t u16;
    uint8_t u8;

    while (n >= 2) {
        u16 = *(s++);
        *(d++) = u16;
        n -= 2;
    }

    if (n == 1) {
        u8 = *((uint8_t *)s);
        *((uint8_t *)d) = u8;
    }

    return dest;
}

char *strcat(char *dest, const char *src) {
    strcpy(dest + strlen(dest), src);
    return dest;
}

void *memset(void *s, int c, size_t n) {
    uint16_t *p = (uint16_t *)s;
    uint16_t cc = ((uint16_t)c << 8) | (uint16_t)c;

    while (n >= 2) {
        *(p++) = cc;
        n -= 2;
    }

    if (n == 1) {
        *((uint8_t *)p) = c;
    }

    return s;
}

int memcmp(const void *s1, const void *s2, size_t n) {
    const uint16_t *p1 = (uint16_t *)s1;
    const uint16_t *p2 = (uint16_t *)s2;
    uint16_t u1;
    uint16_t u2;
    int cmp;

    while (n >= 2) {
        u1 = *(p1++);
        u2 = *(p2++);
        n -= 2;

        if (u1 == u2) {
            continue;
        }

        /* assume little endian */
        cmp = (int)(u1 & 0xff) - (int)(u2 & 0xff);
        if (cmp != 0) {
            return cmp;
        }

        if (u1 < u2) {
            return -1;
        }

        return 1;
    }

    if (n == 1) {
        u1 = *p1;
        u2 = *p2;
        /* assume little endian */
        return (int)(u1 & 0xff) - (int)(u2 & 0xff);
    }

    return 0;
}

int strcmp(const char *s1, const char *s2) {
    const uint16_t *p1 = (uint16_t *)s1;
    const uint16_t *p2 = (uint16_t *)s2;
    uint16_t u1;
    uint16_t u2;
    int cmp;

    for (;;) {
        u1 = *(p1++);
        u2 = *(p2++);

        /* assume little endian */
        cmp = (int)(u1 & 0x00ff) - (int)(u2 & 0x00ff);
        if (cmp != 0) {
            return cmp;
        } else if ((u1 & 0x00ff) == 0x0000) {
            return 0;
        }

        if (u1 < u2) {
            return -1;
        } else if (u1 > u2) {
            return 1;
        } else if ((u1 & 0xff00) == 0x0000) {
            return 0;
        }
    }
}

char *strcpy(char *dest, const char *src) {
    uint16_t *d = (uint16_t *)dest;
    const uint16_t *s = (uint16_t *)src;
    uint16_t u16;
    size_t ret = 0;

    for (;;) {
        u16 = *s;
        /* assume little endian */
        if ((u16 & 0x00ff) == 0x0000) {
            *((char *)d) = '\0';
            break;
        }
        *d = *s;
        d++;
        s++;
        if ((u16 & 0xff00) == 0x0000) {
            break;
        }
    }

    return dest;
}

size_t strlen(const char *s) {
    const uint16_t *p = (uint16_t *)s;
    uint16_t u16;
    size_t ret = 0;

    for (;;) {
        u16 = *p;
        /* assume little endian */
        if ((u16 & 0x00ff) == 0x0000) {
            break;
        } else if ((u16 & 0xff00) == 0x0000) {
            ret++;
            break;
        } else {
            ret += 2;
            p++;
        }
    }

    return ret;
}

void *memchr(const void *s, int c, size_t n) {
    const uint16_t *p = (uint16_t *)s;
    const uint16_t c1 = (uint16_t)c & 0x00ff;
    const uint16_t c2 = ((uint16_t)c << 8) & 0xff00;
    uint16_t u16;

    while (n >= 2) {
        u16 = *p;
        /* assume little endian */
        if ((u16 & 0x00ff) == 0x0000) {
            return NULL;
        } else if ((u16 & 0x00ff) == c1) {
            return (char *)p;
        } else if ((u16 & 0xff00) == 0x0000) {
            return NULL;
        } else if ((u16 & 0xff00) == c2) {
            return ((char *)p) + 1;
        } else {
            p++;
            n -= 2;
        }
    }

    if (n == 1 && *((uint8_t *)p) == (uint8_t)c) {
        return (void *)p;
    }

    return NULL;
}

#endif /* __INLINE_FUNCTIONS__ */

char *strrchr(const char *s, int c) {
    char *ret = NULL;
    const uint16_t *p = (uint16_t *)s;
    const uint16_t c1 = (uint16_t)c & 0x00ff;
    const uint16_t c2 = ((uint16_t)c << 8) & 0xff00;
    uint16_t u16;

    for (;;) {
        u16 = *p;
        /* assume little endian */
        if ((u16 & 0x00ff) == 0x0000) {
            return ret;
        } else if ((u16 & 0x00ff) == c1) {
            ret = (char *)p;
        }

        if ((u16 & 0xff00) == 0x0000) {
            return ret;
        } else if ((u16 & 0xff00) == c2) {
            ret = ((char *)p) + 1;
        }

        p++;
    }

    return ret;
}

/*
 * Implementation is not 100% correct as dest is not padded with NUL bytes
 * if length of src is less than n; but who actually needs this...
 */
char *strncpy(char *dest, const char *src, size_t n) {
    uint16_t *d = (uint16_t *)dest;
    const uint16_t *s = (uint16_t *)src;
    uint16_t u16;
    size_t ret = 0;

    while (n > 0) {
        u16 = *s;
        /* assume little endian */
        if ((u16 & 0x00ff) == 0x0000) {
            *((char *)d) = '\0';
            break;
        }
        if (n == 1) {
            *((char *)d) = u16 & 0x00ff;
            break;
        }
        *d = *s;
        d++;
        s++;
        n -= 2;
        if ((u16 & 0xff00) == 0x0000) {
            break;
        }
    }

    return dest;
}

char *strpbrk(const char *s, const char *accept) {
    const uint16_t *p = (uint16_t *)s;
    const char *c;
    uint16_t c1;
    uint16_t c2;
    uint16_t u16;

    for (;;) {
        u16 = *p;

        /* assume little endian */
        if ((u16 & 0x00ff) == 0x0000) {
            return NULL;
        }

        for (c = accept; *c != '\0'; c++) {
            c1 = (uint16_t)(*c) & 0x00ff;
            c2 = ((uint16_t)(*c) << 8) & 0xff00;

            /* assume little endian */
            if ((u16 & 0x00ff) == c1) {
                return (char *)p;
            } else if ((u16 & 0xff00) == c2) {
                return ((char *)p) + 1;
            }
        }

        /* assume little endian */
        if ((u16 & 0xff00) == 0x0000) {
            return NULL;
        }

        p++;
    }
}

size_t strspn(const char *s, const char *accept) {
    size_t ret = (size_t)0;
    const uint16_t *p = (uint16_t *)s;
    const char *c;
    uint16_t c1;
    uint16_t c2;
    uint16_t u16;
    char found;

    for (;;) {
        u16 = *p;

        /* assume little endian */
        if ((u16 & 0x00ff) == 0x0000) {
            return ret;
        }

        found = 0;

        for (c = accept; *c != '\0'; c++) {
            c1 = (uint16_t)(*c) & 0x00ff;
            c2 = ((uint16_t)(*c) << 8) & 0xff00;

            /* assume little endian */
            if ((u16 & 0x00ff) == c1) {
                if (found == 2) {
                    /* Found both */
                    found = 3;
                    break;
                }
                found = 1;
            }

            if ((u16 & 0xff00) == c2) {
                if (found == 1) {
                    /* Found both */
                    found = 3;
                    break;
                }
                found = 2;
            }
        }

        /* assume little endian */
        if ((u16 & 0xff00) == 0x0000) {
            return ret + found;
        }

        if (found != 3) {
            break;
        }

        ret += 2;
        p++;
    }

    if (found == 1) {
        return ret + 1;
    }

    return ret;
}

char *strstr(const char *haystack, const char *needle) {
    size_t haystacklen, needlelen, c;

    haystacklen = strlen(haystack);
    needlelen = strlen(needle);

    if (needlelen > haystacklen) {
        return NULL;
    }

    if (needlelen == 0) {
        return (char *)haystack;
    }

    c = (haystacklen - needlelen) + 1;

    while (c > 0) {
        if (memcmp(haystack, needle, needlelen) == 0) {
            return (char *)haystack;
        }
        haystack++;
        c--;
    }

    return NULL;
}
